#include <jni.h>
#include <string>
#include <thread>
#include <mutex>
#include <unistd.h>
#include "IMUReader.h"
#include "gyro2quat.h"
#include "native-lib.h"


    bool threadRun = true;
    std::mutex mLock;
    float quat[4] = {0.f};
std::thread mThread;

    void ThreadMain(){
        float time = 2.5f / 1000;
        quat[0] = 1.f;
        quat[1] = 0.f;
        quat[2] = 0.f;
        quat[3] = 0.f;

        if( node_open() != 0) return;

        while(threadRun){
            usleep(1000 * 2.5);

            NodeData current = node_read();
            float gyro[] = {current.gyro_x,current.gyro_y,current.gyro_z};

            mLock.lock();
            applyGyroIntegration(gyro,time,quat);
            mLock.unlock();

            PRINT("applyGyroIntegration return : %f %f %f %f ",quat[0],quat[1],quat[2],quat[3]);

        }
        node_close();
    }

    void StartSensorThread(){
        threadRun = true;

        mThread = std::thread(ThreadMain);

    }

    void StopSensorThread(){

        threadRun = false;
    }

    Orientation GetHeadPose(){
        mLock.lock();
        Orientation ori;
        ori.w = quat[0];
        ori.x = quat[1];
        ori.y = quat[2];
        ori.z = quat[3];
        mLock.unlock();
        return ori;
    }


extern "C" JNIEXPORT jstring JNICALL
Java_com_john_imureader_MainActivity_stringFromJNI(
        JNIEnv* env,
        jobject /* this */) {
    std::string hello = "Hello from C++";


    return env->NewStringUTF(hello.c_str());
}

extern "C" JNIEXPORT void JNICALL
Java_com_john_imureader_MainActivity_startIMU(
        JNIEnv* env,
        jobject /* this */) {
    StartSensorThread();
}

extern "C" JNIEXPORT void JNICALL
Java_com_john_imureader_MainActivity_stopIMU(
        JNIEnv* env,
jobject /* this */) {
    ::StopSensorThread();
}